#include "../inc/msgQ.h"

static OS_Q             *OSQFreeList;              /* Pointer to list of free QUEUE control blocks    */
static OS_Q              OSQTbl[OS_MAX_QS];        /* Table of QUEUE control blocks                 */

/*
*********************************************************************************************************
*                                      QUEUE MODULE INITIALIZATION
*
* Description : this function is called in the inital work
*
* Arguments   :  none
*
* Returns     : none
*
* Note(s)    : when the program is running. this function should be not called
*********************************************************************************************************
*/
void  OS_QInit (void)
{
#if OS_MAX_QS == 1
    OSQFreeList         = &OSQTbl[0];            /* Only ONE queue!                                    */
    OSQFreeList->OSQPtr = (OS_Q *)0;
#endif

#if OS_MAX_QS >= 2
    INT16U  i;
    OS_Q   *pq1;
    OS_Q   *pq2;

    vos_MemSemaphoreTake();

    pq1 = &OSQTbl[0];
    pq2 = &OSQTbl[1];
    for (i = 0; i < (OS_MAX_QS - 1); i++) {      /* Init. list of free QUEUE control blocks            */
        pq1->OSQPtr = pq2;
        pq1++;
        pq2++;
    }
    pq1->OSQPtr = (OS_Q *)0;
    OSQFreeList = &OSQTbl[0];

    vos_MemSemaphoreGive();
#endif
}
/*
*********************************************************************************************************
*                                        CREATE A MESSAGE QUEUE
*
* Description: This function creates a message queue if free queue control blocks are available.
*
* Arguments  : start         is a pointer to the base address of the message queue storage area.  The
*                            storage area MUST be declared as an array of pointers to 'void' as follows
*
*                            void *MessageStorage[size]
*
*              size          is the number of elements in the storage area
*
* Returns    : != (OS_EVENT *)0  is a pointer to the event control clock (OS_EVENT) associated with the
*                                created queue
*              == (OS_EVENT *)0  if no event control blocks were available or an error was detected
*********************************************************************************************************
*/
void  *OSQCreate (void **start, INT16U size)
{
    OS_Q      *pq;

    vos_MemSemaphoreTake();

    pq = OSQFreeList;                        /* Get a free queue control block                     */
    if (pq != (OS_Q *)0) {                   /* Were we able to get a queue control block ?        */
        OSQFreeList         = OSQFreeList->OSQPtr;    /* Yes, Adjust free list pointer to next free*/
        pq->OSQStart        = start;                  /*      Initialize the queue                 */
        pq->OSQEnd          = &start[size];
        pq->OSQIn           = start;
        pq->OSQOut          = start;
        pq->OSQSize         = size;
        pq->OSQEntries      = 0;
    } else {
        pq = (void *)0;
    }

    vos_MemSemaphoreGive();

    return (pq);
}

/*
*********************************************************************************************************
*                                        POST MESSAGE TO A QUEUE
*
* Description: This function sends a message to a queue.  This call has been added to reduce code size
*              since it can replace both OSQPost() and OSQPostFront().  Also, this function adds the
*              capability to broadcast a message to ALL tasks waiting on the message queue.
*
* Arguments  : pevent        is a pointer to the event control block associated with the desired queue
*
*              msg           is a pointer to the message to send.  You MUST NOT send a NULL pointer.
*
*              opt           determines the type of POST performed:
*                            OS_POST_OPT_NONE         POST to a single waiting task
*                                                     (Identical to OSQPost())
*                            OS_POST_OPT_FRONT        POST as LIFO (Simulates OSQPostFront())
*
*                            Below is a list of ALL the possible combination of these flags:
*
*                                 1) OS_POST_OPT_NONE
*                                    identical to OSQPost()
*
*                                 2) OS_POST_OPT_FRONT
*                                    identical to OSQPostFront()
*
* Returns    : OK                    The call was successful and the message was sent
*              OS_Q_FULL             If the queue cannot accept any more messages because it is full.
*
* Warning    : Interrupts can be disabled for a long time if you do a 'broadcast'.  In fact, the
*              interrupt disable time is proportional to the number of tasks waiting on the queue.
*********************************************************************************************************
*/
INT8U  OSQPostOpt (void *pevent, void *msg, INT8U opt)
{
    OS_Q      *pq;


    if (pevent == (void *)0) {                    /* Validate 'pevent'                             */
        return ERROR;
    }
    if (msg == (void *)0) {                           /* Make sure we are not posting a NULL pointer   */
        return (ERROR);
    }

    pq = (OS_Q *)pevent;                              /* Point to queue control block                  */
    if (pq->OSQEntries >= pq->OSQSize) {              /* Make sure queue is not full                   */
        return (OS_Q_FULL);
    }

    vos_MemSemaphoreTake();

    if ((opt & OS_POST_OPT_FRONT) != 0x00) {          /* Do we post to the FRONT of the queue?         */
        if (pq->OSQOut == pq->OSQStart) {             /* Yes, Post as LIFO, Wrap OUT pointer if we ... */
            pq->OSQOut = pq->OSQEnd;                  /*      ... are at the 1st queue entry           */
        }
        pq->OSQOut--;
        *pq->OSQOut = msg;                            /*      Insert message into queue                */
    } else {                                          /* No,  Post as FIFO                             */
        *pq->OSQIn++ = msg;                           /*      Insert message into queue                */
        if (pq->OSQIn == pq->OSQEnd) {                /*      Wrap IN ptr if we are at end of queue    */
            pq->OSQIn = pq->OSQStart;
        }
    }
    pq->OSQEntries++;                                 /* Update the nbr of entries in the queue        */

    vos_MemSemaphoreGive();

    return (OK);
}
/*
*********************************************************************************************************
*                                      ACCEPT MESSAGE FROM QUEUE
*
* Description: This function checks the queue to see if a message is available.  Unlike OSQPend(),
*              OSQAccept() does not suspend the calling task if a message is not available.
*
* Arguments  : pevent        is a pointer to the event control block
*
* Returns    : != (void *)0  is the message in the queue if one is available.  The message is removed
*                            from the so the next time OSQAccept() is called, the queue will contain
*                            one less entry.
*              == (void *)0  if the queue is empty or,
*                            if 'pevent' is a NULL pointer or,
*                            if you passed an invalid event type
*********************************************************************************************************
*/

void  *OSQAccept (void *pevent)
{
    void      *msg;
    OS_Q      *pq;



    if (pevent == (void *)0) {               /* Validate 'pevent'                                  */
        return ((void *)0);
    }

    vos_MemSemaphoreTake();

    pq = (OS_Q *)pevent;                        /* Point at queue control block                       */
    if (pq->OSQEntries > 0) {                    /* See if any messages in the queue                   */
        msg = *pq->OSQOut++;                     /* Yes, extract oldest message from the queue         */
        pq->OSQEntries--;                        /* Update the number of entries in the queue          */
        if (pq->OSQOut == pq->OSQEnd) {          /* Wrap OUT pointer if we are at the end of the queue */
            pq->OSQOut = pq->OSQStart;
        }
    } else {
        msg = (void *)0;                         /* Queue is empty                                     */
    }

    vos_MemSemaphoreGive();
    return (msg);                                /* Return message received (or NULL)                  */
}
/*
*********************************************************************************************************
*                                           FLUSH QUEUE
*
* Description : This function is used to flush the contents of the message queue.
*
* Arguments   : none
*
* Returns     : OS_NO_ERR           upon success
*               OS_ERR_EVENT_TYPE   If you didn't pass a pointer to a queue
*               OS_ERR_PEVENT_NULL  If 'pevent' is a NULL pointer
*********************************************************************************************************
*/

INT8U  OSQFlush (void *pevent)
{
    OS_Q      *pq;


    if (pevent == (void *)0) {                    /* Validate 'pevent'                             */
        return ERROR;
    }

    vos_MemSemaphoreTake();

    pq             = (OS_Q *)pevent;      /* Point to queue storage structure              */
    pq->OSQIn      = pq->OSQStart;
    pq->OSQOut     = pq->OSQStart;
    pq->OSQEntries = 0;

    vos_MemSemaphoreGive();

    return (OK);
}


INT8U  OSQDel (void *pevent)
{

    OS_Q      *pq;

    if (pevent == (void *)0) {                         /* Validate 'pevent'                        */
        return ERROR;
    }

    vos_MemSemaphoreTake();

    pq                  = (OS_Q *)pevent;  /* Return OS_Q to free list        */
    pq->OSQPtr          = OSQFreeList;
    OSQFreeList         = pq;

    vos_MemSemaphoreGive();

    return OK;                   /* Queue has been deleted                   */
}
